#ros lib
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool

#commom lib
import math
import numpy as np
import time
from yahboomcar_laser.common import *
from enum import Enum

print ("import done")
RAD2DEG = 180 / math.pi

class AvoidanceState(Enum):
   SLOW_MONITORING = "slow_monitoring"    # 缓速监控
   NORMAL_FORWARD = "normal_forward"      # 正常行驶  
   TURN_LEFT = "turn_left"               # 左转
   TURN_RIGHT = "turn_right"             # 右转

class laserAvoid(Node):
   def __init__(self, name):
       super().__init__(name)
       
       # ROS通讯 - 保持原有逻辑
       self.sub_laser = self.create_subscription(LaserScan, "/scan", self.registerScan, 1)
       self.sub_JoyState = self.create_subscription(Bool, '/JoyState', self.JoyStateCallback, 1)
       self.pub_vel = self.create_publisher(Twist, '/cmd_vel', 1)
       self.pub_buzzer = self.create_publisher(Bool, '/Buzzer', 1)
       
       # 参数声明 - 保持原有逻辑
       self.declare_parameter("linear", 0.5)
       self.linear = self.get_parameter('linear').get_parameter_value().double_value
       self.declare_parameter("angular", 1.0)
       self.angular = self.get_parameter('angular').get_parameter_value().double_value
       self.declare_parameter("LaserAngle", 40.0)
       self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
       self.declare_parameter("ResponseDist", 0.55)
       self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
       self.declare_parameter("Switch", False)
       self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
       
       # 障碍物检测 - 保持原有逻辑
       self.Right_warning = 0      # 紧急避障阈值检测
       self.Left_warning = 0
       self.front_warning = 0
       
       # 缓速监控检测 - 更大的检测距离
       self.Right_slow_warning = 0   # 缓速模式阈值检测
       self.Left_slow_warning = 0
       self.front_slow_warning = 0
       
       # 新增：左右威胁度评估（仅用于转向决策）
       self.Right_threat_score = 0.0
       self.Left_threat_score = 0.0
       
       self.Joy_active = False
       self.ros_ctrl = SinglePID()
       
       # 状态机相关
       self.current_state = AvoidanceState.SLOW_MONITORING
       self.state_start_time = time.time()
       self.last_laser_data = None
       
       # 定时器
       self.timer = self.create_timer(0.01, self.on_timer)
       self.state_timer = self.create_timer(0.1, self.state_machine_update)  # 10Hz状态机
       
       # 后退转移相关
       self.is_backing = False
       self.back_start_time = 0.0
       
       # 距离阈值设置
       self.emergency_dist = self.ResponseDist * 1    # 0.55米 - 紧急避障距离
       self.slow_monitor_dist = self.ResponseDist * 2.5 # 1.375米 - 缓速监控距离
       
   def on_timer(self):
       """参数更新定时器 - 保持原有逻辑"""
       self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
       self.angular = self.get_parameter('angular').get_parameter_value().double_value
       self.linear = self.get_parameter('linear').get_parameter_value().double_value
       self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
       self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
       
       # 更新距离阈值
       self.emergency_dist = self.ResponseDist * 1    # 紧急避障距离
       self.slow_monitor_dist = self.ResponseDist * 2.5 # 缓速监控距离

   def JoyStateCallback(self, msg):
       """手柄状态回调 - 保持原有逻辑"""
       if not isinstance(msg, Bool): return
       self.Joy_active = msg.data

   def registerScan(self, scan_data):
       """激光雷达数据处理 - 双重阈值检测 + 威胁度计算"""
       if not isinstance(scan_data, LaserScan): return
       
       # 存储最新的激光数据供状态机使用
       self.last_laser_data = scan_data
       
       # 障碍物检测逻辑 - 双重阈值
       ranges = np.array(scan_data.ranges)
       
       # 重置计数器
       self.Right_warning = 0
       self.Left_warning = 0
       self.front_warning = 0
       self.Right_slow_warning = 0
       self.Left_slow_warning = 0
       self.front_slow_warning = 0
       
       # 重置威胁度（仅计算紧急威胁度用于转向决策）
       self.Right_threat_score = 0.0
       self.Left_threat_score = 0.0
       
       for i in range(len(ranges)):
           angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
           distance = ranges[i]
           
           # 过滤无效距离
           if distance <= 0.1 or distance > 10.0:
               continue
           
           # 右侧区域检测
           if 160 > angle > 180 - self.LaserAngle:
               if distance < self.emergency_dist:        # 紧急避障检测
                   self.Right_warning += 1
                   self.Right_threat_score += 1.0 / distance  # 威胁度累积
               if distance < self.slow_monitor_dist:     # 缓速监控检测
                   self.Right_slow_warning += 1
                   
           # 左侧区域检测
           elif - 160 < angle < self.LaserAngle - 180:
               if distance < self.emergency_dist:        # 紧急避障检测
                   self.Left_warning += 1
                   self.Left_threat_score += 1.0 / distance   # 威胁度累积
               if distance < self.slow_monitor_dist:     # 缓速监控检测
                   self.Left_slow_warning += 1
                   
           # 前方区域检测
           elif abs(angle) > 160:
               if distance <= self.emergency_dist:       # 紧急避障检测
                   self.front_warning += 1
               if distance <= self.slow_monitor_dist:    # 缓速监控检测
                   self.front_slow_warning += 1

   def sound_buzzer(self, enable=True):
       """蜂鸣器控制"""
       buzzer_msg = Bool()
       buzzer_msg.data = enable
       self.pub_buzzer.publish(buzzer_msg)

   def publish_motion(self, linear_x=0.0, angular_z=0.0):
       """发布运动指令"""
       twist = Twist()
       twist.linear.x = linear_x
       twist.angular.z = angular_z
       self.pub_vel.publish(twist)

   def transition_to(self, new_state):
       """状态转换"""
       if new_state != self.current_state:
           print(f"状态转换: {self.current_state.value} -> {new_state.value}")
           self.current_state = new_state
           self.state_start_time = time.time()

   def get_state_duration(self):
       """获取当前状态持续时间"""
       return time.time() - self.state_start_time

   def handle_emergency_backward(self):
       """处理紧急机动过程 - 改为原地左转"""
       if self.is_backing:
           return  # 已经在紧急机动过程中
       
       print("紧急情况：左右前都有障碍，原地左转1秒")
       self.is_backing = True
       self.back_start_time = time.time()
       self.sound_buzzer(True)

   def finish_backward(self):
       """完成紧急机动"""
       self.is_backing = False
       self.sound_buzzer(False)
       self.transition_to(AvoidanceState.SLOW_MONITORING)
       print("紧急机动完成，转入缓速监控")

   def analyze_obstacles_and_decide_next_state(self):
       """分析障碍物情况并决定下一状态 - 使用威胁度进行转向决策"""
       
       # 手柄激活或开关关闭时停止
       if self.Joy_active or self.Switch == True:
           self.publish_motion(0.0, 0.0)
           return self.current_state
       
       # 检查是否需要紧急机动（使用紧急阈值）
       if self.front_warning > 10 and self.Left_warning > 10 and self.Right_warning > 10:
           self.handle_emergency_backward()
           return self.current_state
       
       # 决策逻辑：根据当前状态使用不同的检测阈值
       if self.current_state == AvoidanceState.NORMAL_FORWARD:
           # 正常行驶状态：使用缓速监控阈值，提前发现障碍物
           front_check = self.front_slow_warning > 10
           left_check = self.Left_slow_warning > 10
           right_check = self.Right_slow_warning > 10
       else:
           # 其他状态：使用紧急阈值
           front_check = self.front_warning > 10
           left_check = self.Left_warning > 10
           right_check = self.Right_warning > 10
       
       # 威胁度用于转向决策
       left_threat = self.Left_threat_score
       right_threat = self.Right_threat_score
       
       # 多层if判断 - 使用威胁度评估改进转向选择
       if front_check:  # 前方有障碍
           if left_check and right_check:  # 前+左+右都有障碍
               # 这种情况已在上面的紧急机动中处理
               return AvoidanceState.SLOW_MONITORING
           elif left_check and not right_check:  # 前+左有障碍，右侧安全
               return AvoidanceState.TURN_RIGHT
           elif not left_check and right_check:  # 前+右有障碍，左侧安全
               return AvoidanceState.TURN_LEFT
           else:  # 只有前方有障碍，左右都安全
               # 使用威胁度选择转向方向，选择威胁度较低的一侧
               if left_threat <= right_threat:
                   return AvoidanceState.TURN_LEFT   # 左侧威胁度较低，左转
               else:
                   return AvoidanceState.TURN_RIGHT  # 右侧威胁度较低，右转
       else:  # 前方无障碍
           if left_check and right_check:  # 左+右都有障碍
               # 使用威胁度选择转向方向，选择威胁度较低的一侧
               if left_threat <= right_threat:
                   return AvoidanceState.TURN_LEFT   # 左侧威胁度较低，左转
               else:
                   return AvoidanceState.TURN_RIGHT  # 右侧威胁度较低，右转
           elif left_check and not right_check:  # 只有左侧有障碍
               return AvoidanceState.TURN_RIGHT
           elif not left_check and right_check:  # 只有右侧有障碍
               return AvoidanceState.TURN_LEFT
           else:  # 无障碍
               # 如果当前是缓速监控且无障碍，可以转为正常行驶
               if self.current_state == AvoidanceState.SLOW_MONITORING:
                   return AvoidanceState.NORMAL_FORWARD
               else:
                   return AvoidanceState.NORMAL_FORWARD
       
       return self.current_state

   def state_machine_update(self):
       """状态机主循环"""
       # 检查紧急机动
       if self.is_backing:
           if time.time() - self.back_start_time >= 1.0:
               self.finish_backward()
           else:
               # 继续原地左转 - 慢速旋转
               self.publish_motion(linear_x=0.0, angular_z=self.angular * 0.5)
           return
           
       if self.last_laser_data is None:  # 没有激光数据时保持当前状态
           return

       # 分析障碍物并决定下一状态
       next_state = self.analyze_obstacles_and_decide_next_state()
       
       # 检查转弯超时（3秒）
       if (self.current_state in [AvoidanceState.TURN_LEFT, AvoidanceState.TURN_RIGHT] 
           and self.get_state_duration() > 3.0):
           next_state = AvoidanceState.SLOW_MONITORING
           print(f"{self.current_state.value} 超时，转入缓速监控")
       
       # 状态转换
       self.transition_to(next_state)
       
       # 执行当前状态对应的动作
       self.execute_current_state()

   def execute_current_state(self):
       """执行当前状态对应的动作"""
       if self.current_state == AvoidanceState.SLOW_MONITORING:
           self.publish_motion(linear_x=self.linear * 0.3, angular_z=0.0)  # 缓速前进
           
       elif self.current_state == AvoidanceState.NORMAL_FORWARD:
           self.publish_motion(linear_x=self.linear, angular_z=0.0)  # 正常速度前进
           
       elif self.current_state == AvoidanceState.TURN_LEFT:
           self.publish_motion(linear_x=0.0, angular_z=self.angular)  # 原地左转
           
       elif self.current_state == AvoidanceState.TURN_RIGHT:
           self.publish_motion(linear_x=0.0, angular_z=-self.angular)  # 原地右转

   def shutdown_cleanup(self):
       """退出时的清理工作"""
       print("正在安全退出...")
       
       # 停止紧急机动过程
       if self.is_backing:
           self.is_backing = False
       
       # 发送停止指令
       for _ in range(3):
           self.publish_motion(0.0, 0.0)
           self.sound_buzzer(False)
           time.sleep(0.05)
       print("已发送停止指令")

def main():
   rclpy.init()
   laser_avoid = laserAvoid("laser_Avoidance_state_machine")
   print("状态机避障启动")
   print(f"紧急避障距离: {laser_avoid.emergency_dist:.3f}m")
   print(f"缓速监控距离: {laser_avoid.slow_monitor_dist:.3f}m")
   try:
       rclpy.spin(laser_avoid)
   except KeyboardInterrupt:
       print("接收到退出信号...")
   finally:
       laser_avoid.shutdown_cleanup()
       laser_avoid.destroy_node()
       rclpy.shutdown()
       print("程序已安全退出")

if __name__ == '__main__':
   main()